/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This generates topo MCL path from the PeLoTe data
* Requires that the map_matcher has been run first.
*
* usage: topo_log [dir]
* 
* Processes the laser_update.txt file in the directory [dir]
* uses the topo_odo.txt to obtain the "correct initial pose"
* Saves the topoMCL.txt
*/ 

///Select the method

#define USE_OCCU_MAP_FILE

#define USE_TOPO
//#define USE_SCAN
//#define USE_TOPOSCAN
#define VISUALIZATION_ON
#define SAVE_RESULT_IMAGE

//#define MAP_FILE_NAME "../../../../res/autmap-koko.txt"

#ifdef USE_OCCU_MAP_FILE
		#define MAP_FILE_NAME "../../35_remapped_result-map.occu"
		#else
		#define MAP_FILE_NAME "../../../../res/autmap-strip2.txt"
#endif


#define REAL_PATH_FILE "pose_measurement.txt"

#ifdef USE_TOPO
#define SAVE_FILENAME "topo-toisto.txt"
#endif

#ifdef USE_SCAN
//#define SAVE_FILENAME "scan-toisto.txt"
#define SAVE_FILENAME "topo-toisto.txt"
#endif

#ifdef USE_TOPOSCAN
//#define SAVE_FILENAME "toposcan-toisto.txt"
#define SAVE_FILENAME "topo-toisto.txt"
#endif

#define NUMBER_OF_RUNS 1
#define NUMBER_OF_REPEATS 1

float particles[NUMBER_OF_RUNS] = {2000};

#include<stdio.h>
#include<stdlib.h>
#include<math.h>

#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "CTopoOccuMCL.h"
#include "COccuMCL.h"
#include "CTopoScanOccuMCL.h"						
#include "JPEGEncoder.hpp"  ///< For saving JPEG
						
								
void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
				encoder.GetJPEGDataSize(), 
				1, f);
fclose(f);
}

/// For reading the data from file
int readSilmuPoseData(FILE *f,mcl::scan &meas, mcl::pose &p){
		float t1,t2;
		int i;
		char c;
		fscanf(f,"%f %f %f %f %f %d\n",&t1,&p.x,&p.y,&t2,&p.a,&i); ///< Pose
		meas.N = 0;
		
		return 0;
}

 int readSilmuPoseDataNuppu(FILE *f,mcl::scan &meas, mcl::pose &p){
		float t1,t2,foo1,foo2;
		int i;
		char c;
		//fscanf(f,"%f %f %f %f %f %d %f %f\n",&t1,&p.x,&p.y,&t2,&p.a,&i,&foo1,&foo2); ///< Pose
		fscanf(f,"%f %f %f %f %f %d %f %f\n",&t1,&foo1,&foo2,&t2,&p.a,&i,&p.x,&p.y); ///< Pose
		meas.N = 0;
		
		return 0;
}


int readLaserOdometryData(FILE *f,mcl::scan &meas, mcl::pose &p){
		float p1,p2,p3;
		int i;
		char c;
		fscanf(f,"%f %f %f %f %f %f ", &p.x,&p.y,&p.a,&p1,&p2,&p3); ///< Pose
		//fscanf(f,"%f %f %f %f %f %f ",&p1,&p2,&p3,&p.x,&p.y,&p.a); ///< SilMU Pose
		for(i=0;i<361;i++){
				fscanf(f,"%f",&meas.r[i]);
				meas.a[i]=0.5*i*M_PI/180.0;
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<361){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
		meas.N = 361;
		return 0;

}



void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	if(i<0 || j<0 || i>Nx) return;
		
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	float v;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
				v = data[Nx*j+i];
				//if(v>0.29) v = 0.9;
				setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
		}
	}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
		for(int ii=i-1;ii<i+1;ii++){
				for(int jj=j-1;jj<j+1;jj++){
						setRGBpix(rgb,ii,jj,Nx,r,g,b);
				}
		}

}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
		float dy = sin(angle);
		float dx = cos(angle);
		float rr=0;
		int i=0;
		uint x,y;
		while(rr<length){
				x = pix + i*dx;
				y = piy - i*dy;
				RGB[3*Nx*y+3*x] = r;
				RGB[3*Nx*y+3*x+1] = g;
				RGB[3*Nx*y+3*x+2] = b;
				i++;
				rr+=resolution;
		}
}

int main(int argc, char *argv[]){
		float grid_size = 45.0;
		
#ifdef USE_TOPO
		CTopoOccuMCL *topo;
#endif

#ifdef USE_SCAN
		COccuMCL *topo;
#endif
#ifdef USE_TOPOSCAN
		CTopoScanOccuMCL *topo;
#endif
		grid_t *origmap;
		grid_t *dataPtr;
		int Nx,Ny;
		unsigned char *rgb=NULL;
		
		srand(time(NULL));
		int rand_num = rand()%1000;
		
		if(argc != 3){
				fprintf(stderr,"\nUsage: ./topo_log [dir] [describtion]\n\n");
				fprintf(stderr,"[dir] - the directory where the laser_update.txt is located\n");
				fprintf(stderr,"[describtion] - short(one word) describtion\n");
				return 0;
		}
		char *directory=argv[1];
		char *describtion = argv[2];
		fprintf(stderr,"Using directory %s\n",directory);
		
		if(chdir(directory)==-1){
				fprintf(stderr,"Invalid directory '%s'!\n",directory);
				return 0;
		}
		
		FILE *fcor = fopen("topo_odo.txt","rt");
		mcl::pose initial_pose;
		fscanf(fcor,"%f %f %f\n",&initial_pose.x,&initial_pose.y,&initial_pose.a);
		fprintf(stderr,"Using initial pose (%.2f %.2f %.2f)\n",initial_pose.x,initial_pose.y,initial_pose.a);
		fclose(fcor);
		
		
		FILE *log = fopen("laser_update.txt","rt");
		if(log==NULL){
				fprintf(stderr,"No such file\n");	
				return 0;
		}
		
		
		bool isImageWindowStarted=false;

		mcl::scan s; ///< for occupancy grid
		s.allocate(361);
		mcl::pose odo_cur, odo_ref,mest;

		int cnt=0;
				
		vector<mcl::pose> path;
		vector<mcl::pose> variance;
		vector<mcl::pose> odometry;
		
		vector< vector<mcl::pose> >	pose_storage;					
		vector< vector<mcl::pose> >	var_storage;					
		
		float time_vec[NUMBER_OF_RUNS];
		
		vector< vector<float> > scans;
		vector<float> scan_tmp;
		
		while(!feof(log)){
				readLaserOdometryData(log,s,odo_cur);
				scan_tmp.clear();
				for(int i=0;i<s.N;i++){
					scan_tmp.push_back(s.r[i]);	
				}
				scans.push_back(scan_tmp);
				odometry.push_back(odo_cur);
		}
		
		
		FILE *corfilu = fopen(REAL_PATH_FILE,"rt");
		vector<mcl::pose> cor_path;
		while(!feof(corfilu)){
			fscanf(corfilu,"%f %f %f\n",&odo_cur.x,&odo_cur.y,&odo_cur.a);
			cor_path.push_back(odo_cur);
		}
		fclose(corfilu);
		int errors = 0;
	
		
		int draw_cnt = 0;
		FILE *topo_info = fopen(SAVE_FILENAME,"at");
		fprintf(topo_info,"%s:",describtion);
		for(int i=0;i<NUMBER_OF_RUNS;i++){
			fprintf(topo_info,"%.3f ",particles[i]);
		}
		//fprintf(topo_info,"\n");
		
		fclose(log);
		
		for(int runs=0;runs<NUMBER_OF_RUNS;runs++){
			
			while(cnt<NUMBER_OF_REPEATS){
				
#ifdef USE_TOPO
				topo = new CTopoOccuMCL();
#endif 
#ifdef USE_SCAN
				topo = new COccuMCL();
#endif
#ifdef USE_TOPOSCAN		
				topo = new CTopoScanOccuMCL();
#endif

#ifdef USE_TOPO
		#ifdef USE_OCCU_MAP_FILE
				topo->initialize(MAP_FILE_NAME, particles[runs],initial_pose,mcl::pose(2.0,2.0,30*M_PI/180),0.85);
		#else
				fprintf(stderr,"########## Initializing TOPO filter ############\n");
				topo->initialize(MAP_FILE_NAME,1.0,particles[runs],initial_pose,mcl::pose(1.0,1.0,M_PI/30),0.1,1.0);
		#endif
#else
		#ifdef USE_OCCU_MAP_FILE
				topo->initialize(MAP_FILE_NAME, particles[runs],initial_pose,mcl::pose(2.0,2.0,30*M_PI/180),10,0,0.85);
		#else
				fprintf(stderr,"########## Initializing SCAN filter ############\n");
				topo->initialize(MAP_FILE_NAME, 1.0,particles[runs], initial_pose,mcl::pose(1,1,M_PI/30),10,0,0.1,1.0);
		#endif
#endif
				topo->O->getDim(Nx,Ny);
				if(rgb==NULL){
						fprintf(stderr,"Initializing RGB Nx=%d, Ny=%d\n",Nx,Ny);
						rgb = (unsigned char *) malloc(3*Nx*Ny);
				}
#ifdef VISUALIZATION_ON
		   if(!isImageWindowStarted){
						///Initialize Image display	
						addImageWindow(Nx, Ny);
						glutImageGUI_Start();
						isImageWindowStarted=true;
			 }
#endif
				
				long int ss0,us0,ss1,us1,ss2,us2,sec,us,ms;
				float fms;
				ownTime_GetTimeOfDay(&ss0,&us0);
				odo_ref = odometry[0];
				for(int i=1;i<odometry.size();i++){
						odo_cur=odometry[i];
						//fprintf(stderr,"scans.size=%d scans[i].size=%d\n",scans.size(),scans[i].size());
						for(int j=0;j<s.N;j++){
								s.r[j]=scans[i][j];
								if(s.r[j]<0 || s.r[j]>25) s.r[j]=0;
						}
						
					
					////////////////////////////////////////////////////////////////////////////
					/// The MCL
					////////////////////////////////////////////////////////////////////////////
					mest.setToDifferentialPose( odo_cur,odo_ref);
#ifdef USE_SCAN
					topo->runMCL(mest, mcl::pose(fabs(mest.x)*0.3+0.05,fabs(mest.y)*0.3+0.05,0.001*fabs(mest.a)), s);
#else
					topo->runMCL(mest, mcl::pose(fabs(mest.x)*0.3,fabs(mest.y)*0.3,0.001*fabs(mest.a)), s);
#endif
					odo_ref = odo_cur;
					
					mcl::pose mp = topo->filt.getDistributionMean(true); ///< The distribution mean
					path.push_back(mp);
					
					if(sqrt( (mp.x-cor_path[i-1].x)*(mp.x-cor_path[i-1].x) +
									(mp.y-cor_path[i-1].y)*(mp.y-cor_path[i-1].y) )>8.0 ){
						errors++;
						fprintf(stderr,"ERROR %d/%d!\n",errors,cnt);
#ifdef SAVE_RESULT_IMAGE
						setRGBmap(rgb,topo->O, 200,255, 200);
						Grid::TPixel pix;
						Grid::TCoord2D ocoord;
						ocoord.x = mp.x;ocoord.y = mp.y;
						pix = topo->O->toPixel(ocoord);
				
						for(int i=0;i<path.size();i+=1){
								ocoord.x = path[i].x;ocoord.y = path[i].y;
								pix = topo->O->toPixel(ocoord);
								setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
						}
						Grid::TCoord2D coord;
								
						for(int i=0;i<topo->filt.NumOfParticles;i++){
								coord.x = topo->filt.Particles[i].x;
								coord.y = topo->filt.Particles[i].y;
								pix = topo->O->toPixel(coord);
								setRGBpix(rgb,pix.x,pix.y, Nx, 255,255, 0);
						}
				
				///Save to file
						char buf[100];
						sprintf(buf,"%d_%d_topo_toisto_%s.jpg",cnt,draw_cnt,describtion);
						saveJPEG(buf, rgb, Nx, Ny);
#endif
						break;
					}
					
#ifdef SAVE_RESULT_IMAGE
					if(i == odometry.size()-1){
							setRGBmap(rgb,topo->O, 200,255, 200);
							Grid::TPixel pix;
							Grid::TCoord2D ocoord;
							ocoord.x = mp.x;ocoord.y = mp.y;
							pix = topo->O->toPixel(ocoord);
						
							for(int i=0;i<path.size();i+=1){
									ocoord.x = path[i].x;ocoord.y = path[i].y;
									pix = topo->O->toPixel(ocoord);
									setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
							}
							Grid::TCoord2D coord;
									
							for(int i=0;i<topo->filt.NumOfParticles;i++){
									coord.x = topo->filt.Particles[i].x;
									coord.y = topo->filt.Particles[i].y;
									pix = topo->O->toPixel(coord);
									setRGBpix(rgb,pix.x,pix.y, Nx, 255,255, 0);
							}
						
						///Save to file
							char buf[100];
							sprintf(buf,"%d_%d_topo_toisto_%s.jpg",cnt,draw_cnt,describtion);
							saveJPEG(buf, rgb, Nx, Ny);
					}
#endif
					
					////////////////////////////////////////////////////////////////////////////
					/// Visualisation
					////////////////////////////////////////////////////////////////////////////
#ifdef VISUALIZATION_ON					
					if(draw_cnt%50==0){
						setRGBmap(rgb,topo->O, 200,255, 200);
						Grid::TPixel pix;
						Grid::TCoord2D ocoord;
						ocoord.x = mp.x;ocoord.y = mp.y;
						pix = topo->O->toPixel(ocoord);
						
						for(int i=0;i<path.size();i+=1){
								ocoord.x = path[i].x;ocoord.y = path[i].y;
								pix = topo->O->toPixel(ocoord);
								setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0 );
						}
						Grid::TCoord2D coord;
									
						for(int i=0;i<topo->filt.NumOfParticles;i++){
								coord.x = topo->filt.Particles[i].x;
								coord.y = topo->filt.Particles[i].y;
								pix = topo->O->toPixel(coord);
								setRGBpix(rgb,pix.x,pix.y, Nx, 255,255, 0);
						}
						
						for(int i=0;i<s.N;i++){
								coord.x = mp.x + s.r[i]*cos(mp.a+s.a[i]);
								coord.y = mp.y + s.r[i]*sin(mp.a+s.a[i]);
								pix = topo->O->toPixel(coord);
								setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0);
						}
						
						topo->getScan(s, mp);
						for(int i=0;i<s.N;i++){
								coord.x = mp.x + s.r[i]*cos(mp.a+s.a[i]);
								coord.y = mp.y + s.r[i]*sin(mp.a+s.a[i]);
								pix = topo->O->toPixel(coord);
								setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,255, 255);
						}

						displayImage(0, rgb,Ny,Nx);
						
						///Save to file
						char buf[100];
						sprintf(buf,"%d_%d_topo_toisto.jpg",draw_cnt);
						saveJPEG(buf, rgb, Nx, Ny);
						
					}
					
					draw_cnt++;
#endif
					////////////////////////////////////////////////////////////////////////////
					/// The end
					////////////////////////////////////////////////////////////////////////////
					
					
				}
				cnt++;
				ownTime_GetTimeOfDay(&ss2,&us2);
				sec = ss2-ss0;us = us2-us0; ms = sec*1000+us/1000;
				fms=(float)sec*1000.0+(float)us/1000.0;
				//time_vec[runs] = fms/cnt;
				fprintf(stderr,"TOTAL::(%.4f) = (%.4f)/scan \n",fms,fms/cnt);
				
				path.clear();
				
				//var_storage.push_back(variance);
				//variance.clear();
				//path.clear();
				//fclose(log);
		
				delete topo; topo=NULL;
			}
			
			fprintf(topo_info,"%d %d",errors,cnt);
			cnt = 0;
			errors = 0;
		}
		fprintf(topo_info,"\n");
		//FILE *topo_info = fopen("topo-toisto_info.txt","wt");
		
		//for(int i=0;i<NUMBER_OF_RUNS;i++){
		//	fprintf(topo_info,"%f ",time_vec[i]);
		//}
		
		fclose(topo_info);
		
		return 0;
}